#ifndef CONTROLLER_H
#define CONTROLLER_H
#include "Signal.h"
#include "IpTNLP.hpp"
#include "IpIpoptApplication.hpp"
#include "IpSolveStatistics.hpp"
using namespace Ipopt;

// ------------------------------------------------------
// class: MPC_Formulation
// A PURE base class for mpc formulation used by IPOPT solver
// In LPCVD project, the concrete MPC formulation should be implemented
// in a derived class of MPC_Formulation.
// ------------------------------------------------------
class MPC_Formulation : public TNLP{
 protected:
  // parameters in the optimization problem
  int num_MV;     // Number of manipulated variables
  int num_para;   // Number of tunable parameters not hard-coded
 public:
  /** default constructor */
  MPC_Formulation();
  MPC_Formulation(string option_file);
  MPC_Formulation(int i_num_MV, int i_num_para);
  /** default destructor */
  virtual ~MPC_Formulation()=0;

  // =====================================================
  // Methods provided by MPC_Formulation to simplify MPC simulation
  // (solve the same optimization problem repeatly with feedback info)

  // Initialize the problem
  virtual void reset()=0;

  // x is the output vector of the controller
  virtual void get_solution(double* x)=0;

  // Parameterize the optimization problem via feedback
  virtual void parameterize(double time,double** Inputs)=0;

  // Set those non-feedback parameters in the optimization problem
  virtual void setPara(int para_id, double para_val)=0;

  int get_num_MV(){return num_MV;};

  // =====================================================
  /**@name Overloaded from TNLP */
  //@{
  /** Method to return some info about the nlp */
  virtual bool get_nlp_info(Index& n, Index& m, Index& nnz_jac_g,
                            Index& nnz_h_lag, IndexStyleEnum& index_style)=0;

  /** Method to return the bounds for my problem */
  virtual bool get_bounds_info(Index n, Number* x_l, Number* x_u,
                               Index m, Number* g_l, Number* g_u)=0;

  /** Method to return the starting point for the algorithm */
  virtual bool get_starting_point(Index n, bool init_x, Number* x,
                                  bool init_z, Number* z_L, Number* z_U,
                                  Index m, bool init_lambda,
                                  Number* lambda)=0;

  /** Method to return the objective value */
  virtual bool eval_f(Index n, const Number* x, bool new_x, Number& obj_value)=0;

  /** Method to return the gradient of the objective */
  virtual bool eval_grad_f(Index n, const Number* x, bool new_x, Number* grad_f)=0;

  /** Method to return the constraint residuals */
  virtual bool eval_g(Index n, const Number* x, bool new_x, Index m, Number* g)=0;

  /** Method to return:
   *   1) The structure of the jacobian (if "values" is NULL)
   *   2) The values of the jacobian (if "values" is not NULL)
   */
  virtual bool eval_jac_g(Index n, const Number* x, bool new_x,
                          Index m, Index nele_jac, Index* iRow, Index *jCol,
                          Number* values)=0;

  /** Method to return:
   *   1) The structure of the hessian of the lagrangian (if "values" is NULL)
   *   2) The values of the hessian of the lagrangian (if "values" is not NULL)
   */
  virtual bool eval_h(Index n, const Number* x, bool new_x,
                      Number obj_factor, Index m, const Number* lambda,
                      bool new_lambda, Index nele_hess, Index* iRow,
                      Index* jCol, Number* values)=0;

  //@}

  /** @name Solution Methods */
  //@{
  /** This method is called when the algorithm is complete so the TNLP can store/write the solution */
  virtual void finalize_solution(SolverReturn status,
                                 Index n, const Number* x, const Number* z_L, const Number* z_U,
                                 Index m, const Number* g, const Number* lambda,
                                 Number obj_value,
				 const IpoptData* ip_data,
				 IpoptCalculatedQuantities* ip_cq)=0;
  //@}

private:
  /**@name Methods to block default compiler methods.
   * The compiler automatically generates the following three methods.
   *  Since the default compiler implementation is generally not what
   *  you want (for all but the most simple classes), we usually 
   *  put the declarations of these methods in the private section
   *  and never implement them. This prevents the compiler from
   *  implementing an incorrect "default" behavior without us
   *  knowing. (See Scott Meyers book, "Effective C++")
   *  
   */
  //@{
  //  MPC_Formulation();
  MPC_Formulation(const MPC_Formulation&);
  MPC_Formulation& operator=(const MPC_Formulation&);
  //@}
};




// -------------------------------------------------------------
class controller_MPC_IPOPT:public block{
 private:
  // Basic MPC paramters
  
  // Parameters for MPC formulation
  MPC_Formulation* problem;
  SmartPtr<TNLP> problem_base;
  SmartPtr<IpoptApplication> solver;
  double* solution;
  ApplicationReturnStatus status;

  // Parameters define the surface profile input
  int mode;
 public:
  controller_MPC_IPOPT();
  controller_MPC_IPOPT(int i_num_input,int iMode,   // Input parameters
		       int i_num_output,            // Output parameters
		       double idt);                 // Controller parameters
  ~controller_MPC_IPOPT();
  int load_formulation(MPC_Formulation* iProblem);
  int setSP(int sp_id,double sp_val);  // set one SP
  int setSP(double* sp_vals);          // set all SP
  int setPara(int para_id, double para_val); // set one parameter
  int setPara(double* para_vals);            // set all parameters
  virtual void reset();
  virtual void update(double sysTime);
};

// --------------------------------------------------------------
class controller_PID:public block
{
  double Kp;
  double Ki;
  double Kd;
  double Setpoint;
  double u;
  double time;
  double dt;
  bool   AntiWindOff;
 public:
  controller_PID(double SP,double Kp,double Ki,double Kd);
  ~controller_PID();
  void update(double sysTime,double v);
  double output();
  virtual void reset();
};
// -------------------------------------------------------------
/*class controller_MPC:public block
{
 private:
  enum SOLVER{FMINSEARCH, IPOPT, MATLAB};
  void* 
 public:
  
};*/


/* =============================================================
The following controllers take surface profiles as input.
==============================================================*/

// -------------------------------------------------------------
class controller_MPC_Matlab:public block
{
public:
	controller_MPC_Matlab(double* Q,double* R);
	~controller_MPC_Matlab();
	void update(double sysTime);
	double output();
};
// -------------------------------------------------------------
class controller_MPC_fminsearch:public block{
 public:
  controller_MPC_fminsearch(double* Q,double* R);
  ~controller_MPC_fminsearch();
  void update(double sysTime);
  double output();
};
// -------------------------------------------------------------

# endif
